//
// File: cor_cal.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 25-Jun-2024 01:36:24
//

// Include Files
#include "cor_cal.h"
#include <cmath>

// Variable Definitions
static double X0;

static double Y0;

static double sig0;

// Function Definitions
//
// Arguments    : double v
//                double w
//                double dt
//                double *X
//                double *Y
//                double *car_angle
// Return Type  : void
//
void cor_cal(double v, double w, double dt, double *X, double *Y,
             double *car_angle)
{
  double b_dt[6];
  double Pk[3];
  //  初始化静态变量
  // P上一级
  // P更新
  b_dt[0] = dt * std::cos(sig0);
  b_dt[3] = 0.0;
  b_dt[1] = dt * std::sin(sig0);
  b_dt[4] = 0.0;
  b_dt[2] = 0.0;
  b_dt[5] = dt;
  Pk[0] = X0;
  Pk[1] = Y0;
  Pk[2] = sig0;
  for (int i{0}; i < 3; i++) {
    Pk[i] += b_dt[i] * v + b_dt[i + 3] * w;
  }
  *X = Pk[0];
  *Y = Pk[1];
  *car_angle = Pk[2];
  // 静态变量更新
  X0 = Pk[0];
  Y0 = Pk[1];
  sig0 = Pk[2];
}

//
// Arguments    : void
// Return Type  : void
//
void cor_cal_init()
{
  X0 = 0.0;
  Y0 = 0.0;
  sig0 = 0.0;
}

//
// File trailer for cor_cal.cpp
//
// [EOF]
//
